package ch.ethz.fcl.input;

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;

/**
 * table boundary in the original image
 * 
 * @author crazyfrog
 * 
 */
public class Coordinate {
	public static int WIDTH = 640;
	public static int HEIGHT = 480;

	// table length is 1,000mm x 1,000mm
	public static int TABLE_LENGTH = 1000;
	public static int TABLE_MIN_X = 123;
	public static int TABLE_MAX_X = 545;
	public static int TABLE_MIN_Y = 36;
	public static int TABLE_MAX_Y = 457;

	public static int TABLE_INBOUND_MIN_X = 130;
	public static int TABLE_INBOUND_MAX_X = 540;
	public static int TABLE_INBOUND_MIN_Y = 50;
	public static int TABLE_INBOUND_MAX_Y = 450;

	public static int TABLE_OUTBOUND_MIN_X = TABLE_INBOUND_MIN_X - 40;
	public static int TABLE_OUTBOUND_MAX_X = TABLE_INBOUND_MAX_X + 40;

	public static int TABLE_WIDTH = TABLE_MAX_X - TABLE_MIN_X + 1;
	public static int TABLE_HEIGHT = TABLE_MAX_Y - TABLE_MIN_Y + 1;

	static Scalar red = new Scalar(0, 0, 255 * 255);

	public static Mat drawTable(Mat img) {
		Mat imgWithBound = new Mat(img.size(), CvType.CV_16UC3);

		if (img.channels() == 1)
			Imgproc.cvtColor(img, imgWithBound, Imgproc.COLOR_GRAY2BGR);
		else if (img.channels() == 3)
			img.copyTo(imgWithBound);

		Core.line(imgWithBound, new Point(TABLE_MIN_X, TABLE_MIN_Y), new Point(
				TABLE_MIN_X, TABLE_MAX_Y), red);
		Core.line(imgWithBound, new Point(TABLE_MIN_X, TABLE_MIN_Y), new Point(
				TABLE_MAX_X, TABLE_MIN_Y), red);
		Core.line(imgWithBound, new Point(TABLE_MAX_X, TABLE_MIN_Y), new Point(
				TABLE_MAX_X, TABLE_MAX_Y), red);
		Core.line(imgWithBound, new Point(TABLE_MIN_X, TABLE_MAX_Y), new Point(
				TABLE_MAX_X, TABLE_MAX_Y), red);

		return imgWithBound;
	}

	public static Mat crop(Mat mat) {
		return mat
				.submat(0, HEIGHT, TABLE_OUTBOUND_MIN_X, TABLE_OUTBOUND_MAX_X);
	}

	/**
	 * 
	 * @param x
	 *            input x coordinate
	 * @param y
	 *            input y coordinate
	 * @param groundDepth
	 *            input depth of that point without box
	 * @param boxDepth
	 *            input depth of that point with box
	 * @return output {x, y} coordinates on the raw image
	 */
	public static double[] groundImagePosition(double x, double y,
			int groundDepth, int height) {
		double[] pos = new double[2];

		pos[0] = (x - WIDTH / 2.0f) / groundDepth * (groundDepth - height)
				+ WIDTH / 2;
		pos[1] = (y - HEIGHT / 2.0f) / groundDepth * (groundDepth - height)
				+ HEIGHT / 2;

		return pos;
	}

	public static double[] getPositionOnTable(double x, double y,
			float tableWidth, float tableHeight) {
		double[] pos = new double[2];

		pos[0] = (x - TABLE_MIN_X) / TABLE_WIDTH * tableWidth;
		pos[1] = (y - TABLE_MIN_Y) / TABLE_HEIGHT * tableHeight;

		return pos;
	}

	public static boolean pointValid(Point p) {
		int x = (int) p.x, y = (int) p.y;
		if (x <= TABLE_OUTBOUND_MIN_X)
			return false;
		if (x >= TABLE_OUTBOUND_MAX_X)
			return false;
		if (y <= 0)
			return false;
		if (y >= HEIGHT)
			return false;
		return true;
	}

	public static boolean pointOnTable(Point p) {
		double x = p.x, y = p.y;
		if (x < TABLE_MIN_X - 10)
			return false;
		if (x > TABLE_MAX_X + 10)
			return false;
		if (y < TABLE_MIN_Y - 10)
			return false;
		if (y > TABLE_MAX_Y + 10)
			return false;
		return true;
	}
}
